diff --git a/arch/arm/mach-socfpga/include/mach/misc.h b/arch/arm/mach-socfpga/include/mach/misc.h
index 649d2f6..8460acb 100644
--- a/arch/arm/mach-socfpga/include/mach/misc.h
+++ b/arch/arm/mach-socfpga/include/mach/misc.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0 */
 /*
- * Copyright (C) 2016-2017 Intel Corporation
+ * Copyright (C) 2016-2021 Intel Corporation
  */
 
 #ifndef _SOCFPGA_MISC_H_
@@ -45,7 +45,12 @@
 #endif
 
 void do_bridge_reset(int enable, unsigned int mask);
+void force_periph_program(unsigned int status);
+bool is_regular_boot_valid(void);
+bool is_periph_program_force(void);
+void set_regular_boot(unsigned int status);
 void socfpga_pl310_clear(void);
 void socfpga_get_managers_addr(void);
+int qspi_flash_software_reset(void);
 
 #endif /* _SOCFPGA_MISC_H_ */
diff --git a/arch/arm/mach-socfpga/include/mach/reset_manager_arria10.h b/arch/arm/mach-socfpga/include/mach/reset_manager_arria10.h
index 19507c2..26faa62 100644
--- a/arch/arm/mach-socfpga/include/mach/reset_manager_arria10.h
+++ b/arch/arm/mach-socfpga/include/mach/reset_manager_arria10.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0 */
 /*
- * Copyright (C) 2016-2017 Intel Corporation
+ * Copyright (C) 2016-2021 Intel Corporation
  */
 
 #ifndef _RESET_MANAGER_ARRIA10_H_
@@ -22,6 +22,7 @@
 #define RSTMGR_A10_PER1MODRST	0x28
 #define RSTMGR_A10_BRGMODRST	0x2c
 #define RSTMGR_A10_SYSMODRST	0x30
+#define RSTMGR_A10_SYSWARMMASK	0x50
 
 #define RSTMGR_CTRL		RSTMGR_A10_CTRL
 
@@ -115,4 +116,7 @@
 #define ALT_RSTMGR_HDSKEN_FPGAHSEN_SET_MSK	BIT(2)
 #define ALT_RSTMGR_HDSKEN_ETRSTALLEN_SET_MSK	BIT(3)
 
+#define ALT_RSTMGR_FPGAMGRWARMMASK_S2F_SET_MSK	BIT(3)
+#define ALT_RSTMGR_SYSWARMMASK_S2F_SET_MSK	BIT(4)
+
 #endif /* _RESET_MANAGER_ARRIA10_H_ */
diff --git a/arch/arm/mach-socfpga/include/mach/system_manager_arria10.h b/arch/arm/mach-socfpga/include/mach/system_manager_arria10.h
index e4fc6d2..0afe63e 100644
--- a/arch/arm/mach-socfpga/include/mach/system_manager_arria10.h
+++ b/arch/arm/mach-socfpga/include/mach/system_manager_arria10.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0 */
 /*
- * Copyright (C) 2016-2017 Intel Corporation <www.intel.com>
+ * Copyright (C) 2016-2021 Intel Corporation <www.intel.com>
  */
 
 #ifndef _SYSTEM_MANAGER_ARRIA10_H_
@@ -31,6 +31,11 @@
 #define SYSMGR_A10_NOC_IDLEACK			0xd0
 #define SYSMGR_A10_NOC_IDLESTATUS		0xd4
 #define SYSMGR_A10_FPGA2SOC_CTRL		0xd8
+#define SYSMGR_A10_ROMCODE_CTRL			0x204
+#define SYSMGR_A10_ROMCODE_INITSWSTATE	0x20C
+#define SYSMGR_A10_ROMCODE_QSPIRESETCOMMAND	0x208
+#define SYSMGR_A10_ISW_HANDOFF_BASE		0x230
+#define SYSMGR_A10_ISW_HANDOFF_7		0x1c
 
 #define SYSMGR_SDMMC				SYSMGR_A10_SDMMC
 
diff --git a/arch/arm/mach-socfpga/misc_arria10.c b/arch/arm/mach-socfpga/misc_arria10.c
index bf97805..0ed2adf 100644
--- a/arch/arm/mach-socfpga/misc_arria10.c
+++ b/arch/arm/mach-socfpga/misc_arria10.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: GPL-2.0
 /*
- * Copyright (C) 2016-2017 Intel Corporation
+ * Copyright (C) 2016-2021 Intel Corporation
  */
 
 #include <altera.h>
@@ -11,6 +11,7 @@
 #include <miiphy.h>
 #include <netdev.h>
 #include <ns16550.h>
+#include <spi_flash.h>
 #include <watchdog.h>
 #include <asm/arch/misc.h>
 #include <asm/arch/pinmux.h>
@@ -21,6 +22,7 @@
 #include <asm/arch/nic301.h>
 #include <asm/io.h>
 #include <asm/pl310.h>
+#include <linux/sizes.h>
 
 #define PINMUX_UART0_TX_SHARED_IO_OFFSET_Q1_3	0x08
 #define PINMUX_UART0_TX_SHARED_IO_OFFSET_Q2_11	0x58
@@ -29,6 +31,13 @@
 #define PINMUX_UART1_TX_SHARED_IO_OFFSET_Q3_7	0x78
 #define PINMUX_UART1_TX_SHARED_IO_OFFSET_Q4_3	0x98
 
+#define REGULAR_BOOT_MAGIC	0xd15ea5e
+#define PERIPH_RBF_PROG_FORCE	0x50455249
+
+#define QSPI_S25FL_SOFT_RESET_COMMAND	0x00f0ff82
+#define QSPI_N25_SOFT_RESET_COMMAND	0x00000001
+#define QSPI_NO_SOFT_RESET		0x00000000
+
 /*
  * FPGA programming support for SoC FPGA Arria 10
  */
@@ -122,3 +131,118 @@
 	else
 		socfpga_bridges_reset();
 }
+
+/*
+ * This function set/unset flag with number "0x50455249" to
+ * handoff register isw_handoff[7] - 0xffd0624c
+ * This flag is used to force periph RBF program regardless FPGA status
+ * and double periph RBF config are needed on some devices or boards to
+ * stabilize the IO config system.
+ */
+void force_periph_program(unsigned int status)
+{
+	if (status)
+		writel(PERIPH_RBF_PROG_FORCE, socfpga_get_sysmgr_addr() +
+		       SYSMGR_A10_ISW_HANDOFF_BASE + SYSMGR_A10_ISW_HANDOFF_7);
+	else
+		writel(0, socfpga_get_sysmgr_addr() +
+		       SYSMGR_A10_ISW_HANDOFF_BASE + SYSMGR_A10_ISW_HANDOFF_7);
+}
+
+/*
+ * This function is used to check whether
+ * handoff register isw_handoff[7] contains
+ * flag for forcing the periph RBF program "0x50455249".
+ */
+bool is_periph_program_force(void)
+{
+	unsigned int status;
+
+	status = readl(socfpga_get_sysmgr_addr() +
+		       SYSMGR_A10_ISW_HANDOFF_BASE + SYSMGR_A10_ISW_HANDOFF_7);
+
+	if (status == PERIPH_RBF_PROG_FORCE)
+		return true;
+	else
+		return false;
+}
+
+/*
+ * This function set/unset magic number "0xd15ea5e" to
+ * handoff register isw_handoff[7] - 0xffd0624c
+ * This magic number is part of boot progress tracking
+ * and it's required for warm reset workaround on MPFE hang issue.
+ */
+void set_regular_boot(unsigned int status)
+{
+	if (status)
+		writel(REGULAR_BOOT_MAGIC, socfpga_get_sysmgr_addr() +
+		       SYSMGR_A10_ISW_HANDOFF_BASE + SYSMGR_A10_ISW_HANDOFF_7);
+	else
+		writel(0, socfpga_get_sysmgr_addr() +
+		       SYSMGR_A10_ISW_HANDOFF_BASE + SYSMGR_A10_ISW_HANDOFF_7);
+}
+
+/*
+ * This function is used to check whether
+ * handoff register isw_handoff[7] contains
+ * magic number "0xd15ea5e".
+ */
+bool is_regular_boot_valid(void)
+{
+	unsigned int status;
+
+	status = readl(socfpga_get_sysmgr_addr() +
+		       SYSMGR_A10_ISW_HANDOFF_BASE + SYSMGR_A10_ISW_HANDOFF_7);
+
+	if (status == REGULAR_BOOT_MAGIC)
+		return true;
+	else
+		return false;
+}
+
+#if IS_ENABLED(CONFIG_CADENCE_QSPI)
+/* This function is used to trigger software reset
+ * to the QSPI flash. On some boards, the QSPI flash reset may
+ * not be connected to the HPS warm reset.
+ */
+int qspi_flash_software_reset(void)
+{
+	struct udevice *flash;
+	int ret;
+
+	/* Get the flash info */
+	ret = spi_flash_probe_bus_cs(CONFIG_SF_DEFAULT_BUS,
+				     CONFIG_SF_DEFAULT_CS,
+				     CONFIG_SF_DEFAULT_SPEED,
+				     CONFIG_SF_DEFAULT_MODE,
+				     &flash);
+
+	if (ret) {
+		debug("Failed to initialize SPI flash at ");
+		debug("%u:%u (error %d)\n", CONFIG_SF_DEFAULT_BUS,
+		      CONFIG_SF_DEFAULT_CS, ret);
+		return -ENODEV;
+	}
+
+	if (!flash)
+		return -EINVAL;
+
+	/*
+	 * QSPI flash software reset command, for the case where
+	 * no HPS reset connected to QSPI flash reset
+	 */
+	if (!memcmp(flash->name, "N25", SZ_1 + SZ_2))
+		writel(QSPI_N25_SOFT_RESET_COMMAND, socfpga_get_sysmgr_addr() +
+		       SYSMGR_A10_ROMCODE_QSPIRESETCOMMAND);
+	else if (!memcmp(flash->name, "S25FL", SZ_1 + SZ_4))
+		writel(QSPI_S25FL_SOFT_RESET_COMMAND,
+		       socfpga_get_sysmgr_addr() +
+		       SYSMGR_A10_ROMCODE_QSPIRESETCOMMAND);
+	else /* No software reset */
+		writel(QSPI_NO_SOFT_RESET, socfpga_get_sysmgr_addr() +
+		       SYSMGR_A10_ROMCODE_QSPIRESETCOMMAND);
+
+	return 0;
+}
+#endif
diff --git a/arch/arm/mach-socfpga/spl_a10.c b/arch/arm/mach-socfpga/spl_a10.c
index ecb656e..d2f454c 100644
--- a/arch/arm/mach-socfpga/spl_a10.c
+++ b/arch/arm/mach-socfpga/spl_a10.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: GPL-2.0+
 /*
- *  Copyright (C) 2012-2019 Altera Corporation <www.altera.com>
+ *  Copyright (C) 2012-2021 Altera Corporation <www.altera.com>
  */
 
 #include <common.h>
@@ -30,8 +30,13 @@
 #include <asm/arch/fpga_manager.h>
 #include <mmc.h>
 #include <memalign.h>
+#include <linux/delay.h>
 
 #define FPGA_BUFSIZ	16 * 1024
+#define FSBL_IMAGE_IS_VALID	0x49535756
+
+#define FSBL_IMAGE_IS_INVALID	0x0
+#define BOOTROM_CONFIGURES_IO_PINMUX	0x3
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -106,6 +111,8 @@
 
 void spl_board_init(void)
 {
+	int ret;
+
 	ALLOC_CACHE_ALIGN_BUFFER(char, buf, FPGA_BUFSIZ);
 
 	/* enable console uart printing */
@@ -116,8 +123,7 @@
 
 	/* If the full FPGA is already loaded, ie.from EPCQ, config fpga pins */
 	if (is_fpgamgr_user_mode()) {
-		int ret = config_pins(gd->fdt_blob, "shared");
-
+		ret = config_pins(gd->fdt_blob, "shared");
 		if (ret)
 			return;
 
@@ -127,11 +133,110 @@
 	} else if (!is_fpgamgr_early_user_mode()) {
 		/* Program IOSSM(early IO release) or full FPGA */
 		fpgamgr_program(buf, FPGA_BUFSIZ, 0);
+
+		/* Skipping double program for combined RBF */
+		if (!is_fpgamgr_user_mode()) {
+			/*
+			 * Expect FPGA entered early user mode, so
+			 * the flag is set to re-program IOSSM
+			 */
+			force_periph_program(true);
+
+			/* Re-program IOSSM to stabilize IO system */
+			fpgamgr_program(buf, FPGA_BUFSIZ, 0);
+
+			force_periph_program(false);
+		}
 	}
 
 	/* If the IOSSM/full FPGA is already loaded, start DDR */
-	if (is_fpgamgr_early_user_mode() || is_fpgamgr_user_mode())
+	if (is_fpgamgr_early_user_mode() || is_fpgamgr_user_mode()) {
+		if (!is_regular_boot_valid()) {
+			/*
+			 * Ensure all signals in stable state before triggering
+			 * warm reset. This value is recommended from stress
+			 * test.
+			 */
+			mdelay(10);
+
+#if IS_ENABLED(CONFIG_CADENCE_QSPI)
+			/*
+			 * Trigger software reset to QSPI flash.
+			 * On some boards, the QSPI flash reset may not be
+			 * connected to the HPS warm reset.
+			 */
+			qspi_flash_software_reset();
+#endif
+
+			ret = readl(socfpga_get_rstmgr_addr() +
+				    RSTMGR_A10_SYSWARMMASK);
+			/*
+			 * Masking s2f & FPGA manager module reset from warm
+			 * reset
+			 */
+			writel(ret & (~(ALT_RSTMGR_SYSWARMMASK_S2F_SET_MSK |
+			       ALT_RSTMGR_FPGAMGRWARMMASK_S2F_SET_MSK)),
+			       socfpga_get_rstmgr_addr() +
+			       RSTMGR_A10_SYSWARMMASK);
+
+			/*
+			 * BootROM will configure both IO and pin mux after a
+			 * warm reset
+			 */
+			ret = readl(socfpga_get_sysmgr_addr() +
+				    SYSMGR_A10_ROMCODE_CTRL);
+			writel(ret | BOOTROM_CONFIGURES_IO_PINMUX,
+			       socfpga_get_sysmgr_addr() +
+			       SYSMGR_A10_ROMCODE_CTRL);
+
+			/*
+			 * Up to here, image is considered valid and should be
+			 * set as valid before warm reset is triggered
+			 */
+			writel(FSBL_IMAGE_IS_VALID, socfpga_get_sysmgr_addr() +
+			       SYSMGR_A10_ROMCODE_INITSWSTATE);
+
+			/*
+			 * Set this flag to scratch register, so that a proper
+			 * boot progress before / after warm reset can be
+			 * tracked by FSBL
+			 */
+			set_regular_boot(true);
+
+			WATCHDOG_RESET();
+
+			reset_cpu();
+		}
+
+		/*
+		 * Reset this flag to scratch register, so that a proper
+		 * boot progress before / after warm reset can be
+		 * tracked by FSBL
+		 */
+		set_regular_boot(false);
+
+		ret = readl(socfpga_get_rstmgr_addr() +
+			    RSTMGR_A10_SYSWARMMASK);
+
+		/*
+		 * Unmasking s2f & FPGA manager module reset from warm
+		 * reset
+		 */
+		writel(ret | ALT_RSTMGR_SYSWARMMASK_S2F_SET_MSK |
+			ALT_RSTMGR_FPGAMGRWARMMASK_S2F_SET_MSK,
+			socfpga_get_rstmgr_addr() + RSTMGR_A10_SYSWARMMASK);
+
+		/*
+		 * Up to here, MPFE hang workaround is considered done and
+		 * should be reset as invalid until FSBL successfully loading
+		 * SSBL, and prepare jumping to SSBL, then only setting as
+		 * valid
+		 */
+		writel(FSBL_IMAGE_IS_INVALID, socfpga_get_sysmgr_addr() +
+		       SYSMGR_A10_ROMCODE_INITSWSTATE);
+
 		ddr_calibration_sequence();
+	}
 
 	if (!is_fpgamgr_user_mode())
 		fpgamgr_program(buf, FPGA_BUFSIZ, 0);
@@ -169,3 +274,10 @@
 	config_dedicated_pins(gd->fdt_blob);
 	WATCHDOG_RESET();
 }
+
+/* board specific function prior loading SSBL / U-Boot proper */
+void spl_board_prepare_for_boot(void)
+{
+	writel(FSBL_IMAGE_IS_VALID, socfpga_get_sysmgr_addr() +
+	       SYSMGR_A10_ROMCODE_INITSWSTATE);
+}
diff --git a/drivers/fpga/socfpga_arria10.c b/drivers/fpga/socfpga_arria10.c
index b992e6f..798e3a3 100644
--- a/drivers/fpga/socfpga_arria10.c
+++ b/drivers/fpga/socfpga_arria10.c
@@ -604,7 +604,8 @@
 
 			if (strstr(uname, "fpga-periph") &&
 				(!is_fpgamgr_early_user_mode() ||
-				is_fpgamgr_user_mode())) {
+				is_fpgamgr_user_mode() ||
+				is_periph_program_force())) {
 				fpga_node_name = uname;
 				printf("FPGA: Start to program ");
 				printf("peripheral/full bitstream ...\n");